
#pragma once
#include <iostream>
#include <lcm/lcm-cpp.hpp>
#include "lcm_msgs/low_state_cartpole_t.hpp"
#include "lcm_msgs/low_cmd_cartpole_t.hpp"
#include <pthread.h>


class CartpoleLcm
{
private:
    lcm::LCM lcm_;
    low_state_cartpole_t recvState_;
    low_cmd_cartpole_t sendCommand_;
    uint64_t last_timestamp = 0;

    pthread_mutex_t recvMutex_;
    pthread_mutex_t sendMutex_;

    pthread_t lcmThread_;
public:
    CartpoleLcm(/* args */);
    ~CartpoleLcm();

    void HandleLowState(const lcm::ReceiveBuffer* rbuf, const std::string& chan, const low_state_cartpole_t* msg);
    void GetRecv(low_state_cartpole_t& state);
    void SetSend(low_cmd_cartpole_t& cmd);
    void Send();

    static void* lcmThreadFunc(void* handler) {
        CartpoleLcm* handlerObject = static_cast<CartpoleLcm*>(handler);
        while (true) {
            handlerObject->lcm_.handle();
        }
        return NULL;
    }

    void startLCMThread() {
        pthread_create(&lcmThread_, NULL, &CartpoleLcm::lcmThreadFunc, this);
    }

    void joinLCMThread() {
        pthread_join(lcmThread_, NULL);
    }
};
